%% solveQuadProg.m
% Load problem from loadProblem.m and then directly set up the matrices for
% QuadProg in Matlab myself rather than putting it into Yalmip. This will
% be useful for porting to C code.

clear all

primal = 0;
dual = 1;

loadProblem()
[A,B,d,Hp,M] = makeMatrices(n,N,dt,rand);

%  eps('single')
options = optimset('Algorithm','interior-point-convex','TolFun',1e-6,'TolX',1e-6,'TolCon',1e-6);

%% Primal Problem

% Some parameters don't change! Woohoo
% Make matrix V 
% V = [zeros(N*n,N*n);eye(N*n)];
%for zone = 1:n
for timestep = 1:N-1
    if timestep == 1
        V = [B{1},zeros(3,n*N-2)];
    else
        V = [V;
             A{1}*V(end-2:end,:)+[zeros(3,(timestep-1)),B{1},zeros(3,n*(N-1)-timestep)]];
    end
end
%end
[mV,nV] = size(V);
assert(nV == (N-1)*n,'wrong width');
assert(mV == 3*(N-1)*n,'wrong height');

V = [V;eye((N-1)*n)];
AQP = [V;-V];

% Make HQP
HQP = zeros(n*(N-1),n*(N-1));
% for zone = 1:n
    for timestep = 1:N-1
        HQP(timestep,timestep) = R;
%         if timestep > 1
%             HQP(timestep-1,timestep) = -R;
%             HQP(timestep,timestep-1) = -R;
%         end
%         if timestep == N-1
%             HQP(timestep,timestep) = R;
%         end
    end
% end

% Make fQP
fQP = zeros(n*(N-1),1);

% Initialize plots
figure(1)
plot(1:tendtemp,Tlo*ones(tendtemp),'r--')
hold on
plot(1:tendtemp,Thi*ones(tendtemp),'r--')
title('Temperature')
figure(2)
plot(1:tendtemp,ulo*ones(tendtemp),'r--')
hold on 
plot(1:tendtemp,uhi*ones(tendtemp),'r--')
title('Control')

if primal
    disp('Running primal problem in quadprog');
    % Initialize the simulation
    x0 = T0;
    U = []; T = T0;
    for t = 1:tendtemp

        % Make whi and wlo
        whi = zeros(4*n*(N-1),1);
        wlo = zeros(4*n*(N-1),1);
        % for zone = 1:n
           for timestep = 1:N-1
               t1 = (timestep-1)*3+1;
               t3 = ((timestep-1)*3+1+2);
               assert(t3-t1 == 2);
              whi(t1:t3) = Thi*ones(3,1) - A{1}^timestep*x0;
              wlo(t1:t3) = Tlo*ones(3,1) - A{1}^timestep*x0;
              for k = 1:timestep
                  whi(t1:t3) = whi(t1:t3)- A{1}^(timestep-k)*[d(mod(k+t-1,tend));0;0];
                  wlo(t1:t3) = wlo(t1:t3) - A{1}^(timestep-k)*[d(mod(k+t-1,tend));0;0];
              end
           end
        % end
        whi(3*n*(N-1)+1:end) = uhi*ones(n*(N-1),1);
        wlo(3*n*(N-1)+1:end) = ulo*ones(n*(N-1),1);
        assert(length(whi)==4*n*(N-1),'whi wrong length');
        assert(length(wlo)==4*n*(N-1),'wlo wrong length');

        bQP = [whi;-wlo];

    %     if t==1 
    %         disp('bQP is...')
    %         bQP
    %     end

        % Solve the quad prog!!
        [u,fval] = quadprog(HQP,fQP,AQP,bQP);

        % Update plots with MPC open loop
        figure(1)
        x = zeros(3,N);
        x(:,1) = x0;
        for k = 1:N-1
            x(:,k+1) = A*x(:,k)+B*u(k)+[d(mod(t+k-1,tend));0;0];
        end
        plot(t+(1:N),x(1,:),'k--');
        figure(2)
        plot(t+(1:N-1),double(u),'k--');


        % Update x0
    %     x0 = A*x0+B*u(1)+d(mod(t,tend));
        x0 = x(:,2);
        u0 = u(1);
    end
else
    disp('Skipping primal problem')
end



%% Dual Problem 

timevector =zeros(tendtemp,1);

if dual
    disp('Solving dual problem using quadprog')
    % Dual Problem Set up

    QDP = AQP*HQP^-1*AQP';
    VQinv = AQP*HQP^-1;
    Qinv = HQP^-1;

    % Initialize plots
    figure(1)
    plot(1:tendtemp,Tlo*ones(tendtemp),'r--')
    hold on
    plot(1:tendtemp,Thi*ones(tendtemp),'r--')
    title('Temperature')
    figure(2)
    plot(1:tendtemp,ulo*ones(tendtemp),'r--')
    hold on 
    plot(1:tendtemp,uhi*ones(tendtemp),'r--')
    title('Control')

    % Initialize the simulation
    avgtime = 0;
    currtime = 0;
    x0 = T0; 
    U = []; T = T0;
    for t = 1:tendtemp

        % Make whi and wlo
        whi = zeros(4*n*(N-1),1);
        wlo = zeros(4*n*(N-1),1);
        % for zone = 1:n
           for timestep = 1:N-1
               t1 = (timestep-1)*3+1;
               t3 = ((timestep-1)*3+1+2);
               assert(t3-t1 == 2);
              whi(t1:t3) = Thi*ones(3,1) - A{1}^timestep*x0;
              wlo(t1:t3) = Tlo*ones(3,1) - A{1}^timestep*x0;
              for k = 1:timestep
                  whi(t1:t3) = whi(t1:t3)- A{1}^(timestep-k)*[d(mod(k+t-1,tend));0;0];
                  wlo(t1:t3) = wlo(t1:t3) - A{1}^(timestep-k)*[d(mod(k+t-1,tend));0;0];
              end
           end
        % end
        whi(3*n*(N-1)+1:end) = uhi*ones(n*(N-1),1);
        wlo(3*n*(N-1)+1:end) = ulo*ones(n*(N-1),1);
        assert(length(whi)==4*n*(N-1),'whi wrong length');
        assert(length(wlo)==4*n*(N-1),'wlo wrong length');

        bQP = [whi;-wlo];

        if t==1 
            disp('bQP is...')
            bQP
        end

        HDP = bQP + VQinv*fQP;

        % All lambda >= 0
        ADP = -eye(8*n*(N-1));
        BDP = zeros(8*n*(N-1),1);

        % Solve the quad prog!!
        tic
        [lambda,fval] = quadprog(QDP,HDP,ADP,BDP,[],[],[],[],[],options)
    %     [lambda,fval] = PQP(QDP,HDP,ADP,BDP)
        u = -Qinv*(fQP+AQP'*single(lambda));
        solvetime = toc



        % Update plots with MPC open loop
        figure(1)
        x = zeros(3,N);
        x(:,1) = x0;
        for k = 1:N-1
            x(:,k+1) = A{1}*x(:,k)+B{1}*u(k)+[d(mod(t+k-1,tend));0;0];
        end
        plot(t+(1:N),x(1,:),'k--');
        figure(2)
        plot(t+(1:N-1),u,'k--');


        % Update x0
    %     x0 = A*x0+B*u(1)+d(mod(t,tend));
        x0 = x(:,2);
        u0 = u(1);

        % Store used variables
        T = [T,x0];
        U = [U;u0];
        
        
        avgtime = avgtime + solvetime/tendtemp;
        timevector(t) = solvetime;

    end
else
    disp('Skipping dual problem')
end

avgtime


% What is max time and min time
min(timevector)
max(timevector)